#include "m_ring_fifo.h"

#include <string.h>
#include <sched.h>

#define min(a, b)       ((a) > (b) ? (b) : (a))
#define fifo_max_depth  (0xffffffff >> 1)

/*
 * +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~+
 * |        cons_tail         prod_head        |
 * |        cons_head         prod_tail        |
 * |            |                 |            |
 * |            v                 v            |
 * | --|-----|*****|*****|*****|*****|-----|-- |
 * +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~+
 */

/* 环形缓冲区结构 */
struct m_ring_fifo_t {
    volatile uint32_t prod_head;    /* 生产者头指针 */
    volatile uint32_t prod_tail;    /* 生产者尾指针 */

    volatile uint32_t cons_head;    /* 消费者头指针 */
    volatile uint32_t cons_tail;    /* 消费者尾指针 */

    uint32_t size;                  /* 缓冲区大小 */
    uint32_t mask;                  /* 缓冲区大小掩码 */

    void     *buf;                  /* 缓冲区指针 */
    uint32_t is_dynamic;            /* 是否使用了动态内存 */
};

static inline uint32_t is_pow_of_2(uint32_t n)
{
    return (0 != n) && (0 == (n & (n - 1)));
}

static inline uint32_t pow2gt(uint32_t x)
{
    --x;

    x |= x >> 1;
    x |= x >> 2;
    x |= x >> 4;
    x |= x >> 8;
    x |= x >> 16;

    return x + 1;
}

static inline uint32_t atomic_cmp_and_swap32(volatile uint32_t *ptr, uint32_t old_val, uint32_t new_val)
{
    return __sync_bool_compare_and_swap(ptr, old_val, new_val);
}

static inline void thread_yield(void)
{
    sched_yield();
}

struct m_ring_fifo_t *m_ring_fifo_init(void *buf, uint32_t size)
{
    struct m_ring_fifo_t *ring;

    if((NULL != buf) && (0 == is_pow_of_2(size)))
    {
        return NULL;
    }

    ring = malloc(sizeof(struct m_ring_fifo_t));
    if(NULL == ring)
    {
        return NULL;
    }

    if(size > fifo_max_depth)
    {
        size = fifo_max_depth;
    }

    size = pow2gt(size);

    if(NULL == buf)
    {
        ring->buf = malloc(size);
        if(NULL == ring->buf)
        {
            free(ring);

            return NULL;
        }
        ring->is_dynamic = 1;
    }
    else
    {
        ring->buf = buf;
        ring->is_dynamic = 0;
    }

    ring->cons_head = ring->prod_head = 0;
    ring->cons_tail = ring->prod_tail = 0;
    ring->size = size;
    ring->mask = size - 1;

    return ring;
}

void m_ring_fifo_destroy(struct m_ring_fifo_t *ring)
{
    if(0 != ring->is_dynamic)
    {
        free(ring->buf);
        ring->buf = NULL;
    }

    free(ring);
}

uint32_t m_ring_fifo_write_sp(struct m_ring_fifo_t *ring, const void *buf, uint32_t len)
{
    uint32_t prod_head, prod_next;
    uint32_t cons_tail;
    uint32_t wlen;
    uint32_t unused;
    uint32_t off, l;

    prod_head = ring->prod_head;
    cons_tail = ring->cons_tail;
    unused = ring->size - (prod_head - cons_tail);
    wlen = min(len, unused);
    if(0 == wlen){ return wlen; }
    prod_next = prod_head + wlen;
    ring->prod_head = prod_next;

    off = prod_head & ring->mask;
    l = min(wlen, ring->size - off);
    memcpy(ring->buf + off, buf, l);
    memcpy(ring->buf, buf + l, wlen - l);

    ring->prod_tail = prod_next;

    return wlen;
}

uint32_t m_ring_fifo_read_sc(struct m_ring_fifo_t *ring, void *buf, uint32_t len)
{
    uint32_t cons_head, cons_next;
    uint32_t prod_tail;
    uint32_t rlen;
    uint32_t used;
    uint32_t off, l;

    cons_head = ring->cons_head;
    prod_tail = ring->prod_tail;
    used = prod_tail - cons_head;
    rlen = min(len, used);
    if(0 == rlen){ return rlen; }
    cons_next = cons_head + rlen;
    ring->cons_head = cons_next;

    off = cons_head & ring->mask;
    l = min(rlen, ring->size - off);
    memcpy(buf, ring->buf + off, l);
    /* memset(ring->buf + off, 0, l); */
    memcpy(buf + l, ring->buf, rlen - l);
    /* memset(ring->buf, 0, rlen - l); */

    ring->cons_tail = cons_next;

    return rlen;
}

uint32_t m_ring_fifo_write_mp(struct m_ring_fifo_t *ring, const void *buf, uint32_t len)
{
    uint32_t prod_head, prod_next;
    uint32_t cons_tail;
    uint32_t success;
    uint32_t wlen;
    uint32_t unused;
    uint32_t off, l;

    do {
        prod_head = ring->prod_head;
        cons_tail = ring->cons_tail;
        unused = ring->size - (prod_head - cons_tail);
        wlen = min(len, unused);
        if(0 == wlen){ return wlen; }
        prod_next = prod_head + wlen;
        success = atomic_cmp_and_swap32(&ring->prod_head, prod_head, prod_next);
    }while(0 == success);

    off = prod_head & ring->mask;
    l = min(wlen, ring->size - off);
    memcpy(ring->buf + off, buf, l);
    memcpy(ring->buf, buf + l, wlen - l);

    /* 等待前面先写的线程处理完成 */
    while(ring->prod_tail != prod_head)thread_yield();
    ring->prod_tail = prod_next;

    return wlen;
}

uint32_t m_ring_fifo_read_mc(struct m_ring_fifo_t *ring, void *buf, uint32_t len)
{
    uint32_t cons_head, cons_next;
    uint32_t prod_tail;
    uint32_t success;
    uint32_t rlen;
    uint32_t used;
    uint32_t off, l;

    do {
        cons_head = ring->cons_head;
        prod_tail = ring->prod_tail;
        used = prod_tail - cons_head;
        rlen = min(len, used);
        if(0 == rlen){ return rlen; }
        cons_next = cons_head + rlen;
        success = atomic_cmp_and_swap32(&ring->cons_head, cons_head, cons_next);
    }while(0 == success);

    off = cons_head & ring->mask;
    l = min(rlen, ring->size - off);
    memcpy(buf, ring->buf + off, l);
    /* memset(ring->buf + off, 0, l); */
    memcpy(buf + l, ring->buf, rlen - l);
    /* memset(ring->buf, 0, rlen - l); */

    /* 等待前面先读的线程处理完成 */
    while(ring->cons_tail != cons_head)thread_yield();
    ring->cons_tail = cons_next;

    return rlen;
}

uint32_t m_ring_fifo_is_full(struct m_ring_fifo_t *ring)
{
    return ring->size == (ring->prod_head - ring->cons_tail);
}

uint32_t m_ring_fifo_is_empty(struct m_ring_fifo_t *ring)
{
    return ring->prod_tail == ring->cons_head;
}

uint32_t m_ring_fifo_avail(struct m_ring_fifo_t *ring)
{
    return ring->size - (ring->prod_head - ring->cons_tail);
}

uint32_t m_ring_fifo_count(struct m_ring_fifo_t *ring)
{
    return ring->prod_tail - ring->cons_head;
}
